#include "motor.h"

#include "C_code_Log.h"
#include "myconfig.h"
#include "pid.h"
#include "tim.h"
#include "stdlib.h"

//记录电机的速度和PWM值的对应关系，速度单位为每0.1s的编码器脉冲数，PWM范围0-2000并与之对应的速度
#ifdef FOUR_ENCODER
struct MotorSpeedPWMInfo leftFrontMotorSpeedPWMInfo[] = {{},{},{}};
struct MotorSpeedPWMInfo leftBackMotorSpeedPWMInfo[] = {{},{},{}};
struct MotorSpeedPWMInfo rightFrontMotorSpeedPWMInfo[] = {{},{},{}};
struct MotorSpeedPWMInfo rightBackMotorSpeedPWMInfo[] = {{},{},{}};
#else 
struct MotorSpeedPWMInfo leftMotorSpeedPWMInfo[] = {{0,0},{32,500},{65,800},{98,1100},{133,1400},{168,1700},{199,2000}};
struct MotorSpeedPWMInfo rightMotorSpeedPWMInfo[] = {{0,0},{32,500},{65,800},{98,1100},{130,1400},{163,1700},{199,2000}};

#endif // 

// 规划：1、有多种驱动模式
// 1、编码器速度固定输出，用PID调节，优点：速度稳定，可以更换电机，缺点：到达目标速度时间较长（3个时间周期，一个为0.1秒），不适合快速变速，需要定时器
// 2、PWM固定输出，优点是速度快，兼容旧代码，缺点是左右轮速度不一样，直走不正，要手动调整左右的PWM差值，不需要定时器
// 3、PWM给定目标值，但是会用编码器调整4个电机速度或者左右两边速度一致，相当于PWM固定输出的改良版，需要定时器
motorSpeedTypeDef gl_motor;
DEKE_TypeDef Motor_handle[4];
#define LEFT_FRONT  0
#define LEFT_BACK   2
#define RIGHT_FRONT 1
#define RIGHT_BACK  3
DEKE_CONFIG_T Motor_config[4] = {
  {.encoder_handle = &LEFT_FRONT_ENCODER_TIM_HANDLE,.other_motor_encoder = NULL,.PWM_handle = &LEFT_TIM_HANDLE,.tim_channel_1 = LEFT_FRONT_TIM_CHANNEL_1,.tim_channel_2 = LEFT_FRONT_TIM_CHANNEL_2},
  {.encoder_handle = &RIGHT_FRONT_ENCODER_TIM_HANDLE,.other_motor_encoder = NULL,.PWM_handle = &RIGHT_TIM_HANDLE,.tim_channel_1 = RIGHT_FRONT_TIM_CHANNEL_1,.tim_channel_2 = RIGHT_FRONT_TIM_CHANNEL_2},
#ifdef FOUR_ENCODER
  {.encoder_handle = &LEFT_BACK_ENCODER_TIM_HANDLE,.PWM_handle = &LEFT_TIM_HANDLE,.tim_channel_1 = LEFT_BACK_TIM_CHANNEL_1,.tim_channel_2 = LEFT_BACK_TIM_CHANNEL_2},
  {.encoder_handle = &RIGHT_BACK_ENCODER_TIM_HANDLE,.PWM_handle = &RIGHT_TIM_HANDLE,.tim_channel_1 = RIGHT_BACK_TIM_CHANNEL_1,.tim_channel_2 = RIGHT_BACK_TIM_CHANNEL_2},
#else // FOUR_ENCODER
  {.encoder_handle = NULL,.other_motor_encoder = &Motor_handle[LEFT_FRONT],.PWM_handle = &LEFT_TIM_HANDLE,.tim_channel_1 = LEFT_BACK_TIM_CHANNEL_1,.tim_channel_2 = LEFT_BACK_TIM_CHANNEL_2},
  {.encoder_handle = NULL,.other_motor_encoder = &Motor_handle[RIGHT_FRONT],.PWM_handle = &RIGHT_TIM_HANDLE,.tim_channel_1 = RIGHT_BACK_TIM_CHANNEL_1,.tim_channel_2 = RIGHT_BACK_TIM_CHANNEL_2},
#endif
};
// static PIDTypeDef* pidLeftFront;   // 左前轮PID结构体
// static PIDTypeDef* pidRightFront;  // 右前轮PID结构体
// static PIDTypeDef* pidLeftBack;   // 左后轮PID结构体
// static PIDTypeDef* pidRightBack;  // 右后轮PID结构体
// motorPWMTypeDef gl_motorPWM;
// 电机PID调节观察者，会在定时器6中断中调用，但是要把Update函数改成自己想要的功能
Observer MotorPIDAdjustObserver = {.id = 2,
                                   .update = NULL /*SpeedAdjustment_PID*/};
/// @brief 电机初始化
/// @note 开启PWM通道，把电机PID调节观察者加入定时器6中断中
/// @warning 此时观察者的update函数为空，需要在运行时修改
void MotorInit() {
  DEKE_Init(&Motor_handle[LEFT_FRONT],&Motor_config[LEFT_FRONT],MOTOR_MODE_Direct,Left_wheel,LEFT_FRONT_CHANGE_DIRECTION,LEFT_FRONT_ENCODER_ONE_ROUND);
  DEKE_Init(&Motor_handle[LEFT_BACK],&Motor_config[LEFT_BACK],MOTOR_MODE_Direct,Left_wheel,LEFT_BACK_CHANGE_DIRECTION,LEFT_BACK_ENCODER_ONE_ROUND);
#ifdef FOUR_ENCODER
  DEKE_Init(&Motor_handle[RIGHT_FRONT],&Motor_config[RIGHT_FRONT],MOTOR_MODE_Direct,RIGHT_FRONT_CHANGE_DIRECTION,RIGHT_FRONT_ENCODER_ONE_ROUND,NULL);
  DEKE_Init(&Motor_handle[RIGHT_BACK],&Motor_config[RIGHT_BACK],MOTOR_MODE_Direct,RIGHT_BACK_CHANGE_DIRECTION,RIGHT_BACK_ENCODER_ONE_ROUND,NULL);
#else
  DEKE_Init(&Motor_handle[RIGHT_FRONT],&Motor_config[RIGHT_FRONT],MOTOR_MODE_Direct,Right_wheel,RIGHT_FRONT_CHANGE_DIRECTION,RIGHT_FRONT_ENCODER_ONE_ROUND);
  DEKE_Init(&Motor_handle[RIGHT_BACK],&Motor_config[RIGHT_BACK],MOTOR_MODE_Direct,Right_wheel,RIGHT_BACK_CHANGE_DIRECTION,RIGHT_BACK_ENCODER_ONE_ROUND);
#endif

  // pidLeftFront = Motor_handle[LEFT_FRONT].pid_handle;
  // pidRightFront = Motor_handle[RIGHT_FRONT].pid_handle;
  // pidLeftBack = Motor_handle[LEFT_BACK].pid_handle;
  // pidRightBack = Motor_handle[RIGHT_BACK].pid_handle;

  gl_TIM6_IT.add(&gl_TIM6_IT, &MotorPIDAdjustObserver);
}

/// @brief 设置电机结构体的速度
/// @param motor 电机结构体
/// @param leftSpeed 左轮速度
/// @param rightSpeed 右轮速度
/// @note 只是改变结构体的设定速度，不会改变实际速度
static void setTypeDefSpeed(motorSpeedTypeDef *motor, int16_t leftSpeed,
                            int16_t rightSpeed) {
  motor->leftSpeed = leftSpeed;
  motor->rightSpeed = rightSpeed;
}

#ifdef FOUR_WHEEL
//获取各个电机的PWM值
int16_t getLeftFrontMotorPWM() { return gl_motor.leftFrontPWM; }
int16_t getLeftBackMotorPWM() { return gl_motor.leftBackPWM; }
int16_t getRightFrontMotorPWM() { return gl_motor.rightFrontPWM; }
int16_t getRightBackMotorPWM() { return gl_motor.rightBackPWM; }
#endif
/**
 * @brief Get the Left Motor average PWM  
 * 
 * @return int16_t 
 */
int16_t getLeftMotorPWM() { 
#ifdef TWO_WHEEL
  return gl_motor.leftPWM; 
#endif
#ifdef FOUR_WHEEL
  return (getLeftFrontMotorPWM() + getLeftBackMotorPWM()) / 2;
#endif
}
/**
 * @brief Get the Right Motor average PWM  
 * 
 * @return int16_t 
 */
int16_t getRightMotorPWM() {
#ifdef TWO_WHEEL
  return gl_motor.rightPWM;
#endif
#ifdef FOUR_WHEEL
  return (getRightFrontMotorPWM() + getRightBackMotorPWM()) / 2;
#endif
}

//电机转速与PWM值的大致换算
#ifndef FOUR_ENCODER
/**
 * @brief 通过速度获取左右电机PWM值
 * 
 * @param speed 速度，单位为每0.1s的编码器脉冲数
 * @return int16_t 
 */
int16_t getLeftMotorPWMBySpeed(int16_t speed){
    return getMotorPWMBySpeed(leftMotorSpeedPWMInfo,sizeof(leftMotorSpeedPWMInfo)/sizeof(struct MotorSpeedPWMInfo),speed);
}
/**
 * @brief 通过速度获取左右电机PWM值
 * 
 * @param speed 速度，单位为每0.1s的编码器脉冲数
 * @return int16_t 
 */
int16_t getRightMotorPWMBySpeed(int16_t speed){
    return getMotorPWMBySpeed(rightMotorSpeedPWMInfo,sizeof(rightMotorSpeedPWMInfo)/sizeof(struct MotorSpeedPWMInfo),speed);
}
/**
 * @brief 通过PWM获取左电机速度
 * 
 * @param PWM PWM值
 * @return float 
 */
float getLeftMotorSpeedByPWM(int16_t PWM){
    return getMotorSpeedByPWM(leftMotorSpeedPWMInfo,sizeof(leftMotorSpeedPWMInfo)/sizeof(struct MotorSpeedPWMInfo),PWM);
}
/**
 * @brief 通过PWM获取右电机速度
 * 
 * @param PWM PWM值
 * @return float 
 */
float getRightMotorSpeedByPWM(int16_t PWM){
    return getMotorSpeedByPWM(rightMotorSpeedPWMInfo,sizeof(rightMotorSpeedPWMInfo)/sizeof(struct MotorSpeedPWMInfo),PWM);
}
#else
/**
 * @brief 通过速度获取左前电机PWM值
 * 
 * @param speed 速度，单位为每0.1s的编码器脉冲数
 * @return int16_t 
 */
int16_t getLeftFrontMotorPWMBySpeed(int16_t speed){
    return getMotorPWMBySpeed(leftFrontMotorSpeedPWMInfo,sizeof(leftFrontMotorSpeedPWMInfo)/sizeof(struct MotorSpeedPWMInfo),speed);
}
/**
 * @brief 通过速度获取左后电机PWM值
 * 
 * @param speed 速度，单位为每0.1s的编码器脉冲数
 * @return int16_t 
 */
int16_t getLeftBackMotorPWMBySpeed(int16_t speed){
    return getMotorPWMBySpeed(leftBackMotorSpeedPWMInfo,sizeof(leftBackMotorSpeedPWMInfo)/sizeof(struct MotorSpeedPWMInfo),speed);
}
/**
 * @brief 通过速度获取右前电机PWM值
 * 
 * @param speed 速度，单位为每0.1s的编码器脉冲数
 * @return int16_t 
 */
int16_t getRightFrontMotorPWMBySpeed(int16_t speed){
    return getMotorPWMBySpeed(rightFrontMotorSpeedPWMInfo,sizeof(rightFrontMotorSpeedPWMInfo)/sizeof(struct MotorSpeedPWMInfo),speed);
}
/**
 * @brief 通过速度获取右后电机PWM值
 * 
 * @param speed 速度，单位为每0.1s的编码器脉冲数
 * @return int16_t 
 */
int16_t getRightBackMotorPWMBySpeed(int16_t speed){
    return getMotorPWMBySpeed(rightBackMotorSpeedPWMInfo,sizeof(rightBackMotorSpeedPWMInfo)/sizeof(struct MotorSpeedPWMInfo),speed);
}
/**
 * @brief 通过PWM获取左前电机速度
 * 
 * @param PWM PWM值
 * @return float 
 */
float getLeftFrontMotorSpeedByPWM(int16_t PWM){
    return getMotorSpeedByPWM(leftFrontMotorSpeedPWMInfo,sizeof(leftFrontMotorSpeedPWMInfo)/sizeof(struct MotorSpeedPWMInfo),PWM);
}
/**
 * @brief 通过PWM获取左后电机速度
 * 
 * @param PWM PWM值
 * @return float 
 */
float getLeftBackMotorSpeedByPWM(int16_t PWM){
    return getMotorSpeedByPWM(leftBackMotorSpeedPWMInfo,sizeof(leftBackMotorSpeedPWMInfo)/sizeof(struct MotorSpeedPWMInfo),PWM);
}
/**
 * @brief 通过PWM获取右前电机速度
 * 
 * @param PWM PWM值
 * @return float 
 */
float getRightFrontMotorSpeedByPWM(int16_t PWM){
    return getMotorSpeedByPWM(rightFrontMotorSpeedPWMInfo,sizeof(rightFrontMotorSpeedPWMInfo)/sizeof(struct MotorSpeedPWMInfo),PWM);
}
/**
 * @brief 通过PWM获取右后电机速度
 * 
 * @param PWM PWM值
 * @return float 
 */
float getRightBackMotorSpeedByPWM(int16_t PWM){
    return getMotorSpeedByPWM(rightBackMotorSpeedPWMInfo,sizeof(rightBackMotorSpeedPWMInfo)/sizeof(struct MotorSpeedPWMInfo),PWM);
}
#endif


/// @brief 电机固定转速模式
/// @note
/// 第一种驱动模式，编码器速度固定输出，用PID调节，优点：速度稳定，可以更换电机，缺点：到达目标速度时间较长（3个时间周期，一个为0.1秒），不适合快速变速
/// @param motor 电机结构体
/// @note 只对左前轮进行PID调节，未完成
/// @note 新增 2024/1/8 未做完全测试
/// 增加了电机插反检测，如果电机插反了，会停止电机，方法：通过检查连续两次的误差是否在减小来判断电机是否在正确的方向上运行。
/// 如果误差在增大，那么我们可以认为电机可能在错误的方向上运行
void SpeedAdjustment_PID() {
  // 输出 PWM
  DEKE_SpeedAdjustment_PID();
}
/// @brief 用于在运行时修改电机速度的目标值,在电机固定转速模式下使用
/// @param leftSpeed 左轮的目标速度
/// @param rightSpeed 右轮的目标速度
/// @note 未完成
/// @note
/// 不能重新初始化，那样会重新从0开始输出PWM，会导致电机转速突然变小，在运行时想修改电机速度要用这个函数
/// @date 2023/11/12
void resetMotorSpeedValue_runTime(int16_t leftSpeed, int16_t rightSpeed) {
  if (getMotorRunMode() != MOTOR_MODE_SPEED) {
    LOG_ERROR("电机不是固定转速模式,无法使用resetMotorSpeedValue_runTime函数");
    return;
  }
  gl_motor.leftSpeed = leftSpeed;
  gl_motor.rightSpeed = rightSpeed;

  DEKE_SetSpeed(&Motor_handle[LEFT_FRONT],leftSpeed);
  DEKE_SetSpeed(&Motor_handle[LEFT_BACK],leftSpeed);
  DEKE_SetSpeed(&Motor_handle[RIGHT_FRONT],rightSpeed);
  DEKE_SetSpeed(&Motor_handle[RIGHT_BACK],rightSpeed);

//更改左前轮或左轮的PID累计误差，使得电机速度更快的到达目标速度
// #ifndef FOUR_ENCODER
//   int sumError = getLeftMotorPWMBySpeed(gl_motor.leftSpeed) / pidLeftFront->ki;
// #endif // !FOUR_ENCODER
// #ifdef FOUR_ENCODER
//   int sumError = getLeftFrontMotorPWMBySpeed(gl_motor.leftSpeed) / pidLeftFront->ki;
// #endif // FOUR_ENCODER                 
//   if ((pidLeftFront->sumError < sumError && pidLeftFront->setValue < leftSpeed) ||
//       (pidLeftFront->sumError > sumError && pidLeftFront->setValue > leftSpeed)) {
//       pidLeftFront->sumError = sumError;
//   }
//   pidLeftFront->setValue = leftSpeed;

// //更改右前轮或右轮的PID累计误差，使得电机速度更快的到达目标速度
// #ifndef FOUR_ENCODER
//   sumError = getRightMotorPWMBySpeed(gl_motor.rightSpeed) / pidRightFront->ki;
// #endif // !FOUR_ENCODER
// #ifdef FOUR_ENCODER
//   sumError = getRightFrontMotorPWMBySpeed(gl_motor.rightSpeed) / pidRightFront->ki;
// #endif // FOUR_ENCODER        
//   if ((pidRightFront->sumError < sumError && pidRightFront->setValue < rightSpeed) ||
//       (pidRightFront->sumError > sumError && pidRightFront->setValue > rightSpeed)) {
//       pidRightFront->sumError = sumError;
//   }
//   pidRightFront->setValue = rightSpeed;
// #ifdef FOUR_ENCODER
// //更改左后轮的PID累计误差，使得电机速度更快的到达目标速度
//   sumError = getLeftBackMotorPWMBySpeed(gl_motor.leftSpeed) / pidLeftBack->ki;
//   if ((pidLeftBack->sumError < sumError && pidLeftBack->setValue < leftSpeed) ||
//       (pidLeftBack->sumError > sumError && pidLeftBack->setValue > leftSpeed)) {
//     pidLeftBack->sumError = sumError;
//   }
//   pidLeftBack->setValue = leftSpeed;

// //更改右后轮的PID累计误差，使得电机速度更快的到达目标速度
//   sumError = getRightBackMotorPWMBySpeed(gl_motor.rightSpeed) / pidRightBack->ki;
//   if ((pidRightBack->sumError < sumError && pidRightBack->setValue < rightSpeed) ||
//       (pidRightBack->sumError > sumError && pidRightBack->setValue > rightSpeed)) {
//       pidRightBack->sumError = sumError;
//     } 
//   pidRightBack->setValue = rightSpeed;
// #endif  // FOUR_ENCODER
}
/// @brief 用于对旧代码的支持，直接设置电机的速度
/// @param leftPWM 左轮的速度
/// @param rightPWM 右轮的速度
void speedAdjustment(int16_t leftPWM, int16_t rightPWM) {
  if (MotorPIDAdjustObserver.update == NULL) {
#ifdef FOUR_WHEEL
    // 更新电机结构体的PWM值
    gl_motor.leftFrontPWM = leftPWM;
    gl_motor.leftBackPWM = leftPWM;
    gl_motor.rightFrontPWM = rightPWM;
    gl_motor.rightBackPWM = rightPWM;
    DEKE_SetRealSpeed(&Motor_handle[LEFT_FRONT],leftPWM);
    DEKE_SetRealSpeed(&Motor_handle[LEFT_BACK],leftPWM);
    DEKE_SetRealSpeed(&Motor_handle[RIGHT_FRONT],rightPWM);
    DEKE_SetRealSpeed(&Motor_handle[RIGHT_BACK],rightPWM);
#endif  // FOUR_WHEEL

#ifdef TWO_WHEEL
    // 更新电机结构体的PWM值
    gl_motor.leftPWM = leftPWM;
    gl_motor.rightPWM = rightPWM;
    DEKE_SetRealSpeed(&Motor_handle[LEFT_FRONT],leftPWM);
    DEKE_SetRealSpeed(&Motor_handle[RIGHT_FRONT],rightPWM);
#endif  // TWO_WHEEL

  } else {
    LOG_ERROR("电机已经处于固定转速模式,无法使用speedAdjustment函数");
  }
}

/// @brief 用于设置电机的实际速度(通过PWM)
/// @param PWMLeftFront 要设置的左前轮速度（一个PWM值）范围是-8000到8000
/// @param PWMLeftBack 要设置的左后轮速度（一个PWM值）范围是-8000到8000
/// @param PWMRightFort 要设置的右前轮速度（一个PWM值）范围是-8000到8000
/// @param PWMRightBack 要设置的右后轮速度（一个PWM值）范围是-8000到8000
/// @note 可以根据宏定义来改变电机的正方向，可以不用改变电机的接线
/// @note
/// 产生电机驱动的PWM的定时器说明：TIM5通道3和4对应左前，TIM5通道1和2对应左后
void speedAdjustmentForFour(int16_t PWMLeftFront, int16_t PWMLeftBack,
                            int16_t PWMRightFort, int16_t PWMRightBack) {
  if (MotorPIDAdjustObserver.update == NULL) {
#ifdef FOUR_WHEEL
    // 更新电机结构体的PWM值
    gl_motor.leftFrontPWM = PWMLeftFront;
    gl_motor.leftBackPWM = PWMLeftBack;
    gl_motor.rightFrontPWM = PWMRightFort;
    gl_motor.rightBackPWM = PWMRightBack;
    DEKE_SetRealSpeed(&Motor_handle[LEFT_FRONT],PWMLeftFront);
    DEKE_SetRealSpeed(&Motor_handle[LEFT_BACK],PWMLeftBack);
    DEKE_SetRealSpeed(&Motor_handle[RIGHT_FRONT],PWMRightFort);
    DEKE_SetRealSpeed(&Motor_handle[RIGHT_BACK],PWMRightBack);
#endif  // FOUR_WHEEL

#ifdef TWO_WHEEL
    // 更新电机结构体的PWM值
    gl_motor.leftPWM = PWMLeftFront;
    gl_motor.rightPWM = PWMRightFort;
    DEKE_SetRealSpeed(&Motor_handle[LEFT_FRONT],PWMLeftFront);
    DEKE_SetRealSpeed(&Motor_handle[RIGHT_FRONT],PWMRightFort);
#endif  // TWO_WHEEL

  } else {
    LOG_ERROR("电机已经处于固定转速模式,无法使用speedAdjustment函数");
  }
}
/*
 * 函数介绍：调整左右电机的速度差，用于位置式PID
 * 输入参数：leftSpeed(初始左轮速度)，rightSpeed（初速右轮速度），dSpeed（偏差速度）
 * 输出参数：无
 * 返回值  ：无
 * 其他    ：实现让小车能够在设定的速度下稳定跑直线
 */
void positionSpeedOut(int leftSpeed, int rightSpeed, int dSpeed) {
  dSpeed = dSpeed / 2;
  leftSpeed -= dSpeed;
  rightSpeed += dSpeed;

  speedAdjustment(leftSpeed, rightSpeed);
}
/**
 * @brief 前后左右四个轮子的速度调整
 * 
 * @param leftSpeed 左轮基础速度
 * @param rightSpeed 右轮基础速度
 * @param LR_dSpeed 右减左的速度差
 * @param FB_dSpeed 前减后的速度差
 */
void positionSpeedOut_2(int leftSpeed, int rightSpeed, int LR_dSpeed,int FB_dSpeed) {
  LR_dSpeed = LR_dSpeed / 2;
  FB_dSpeed = FB_dSpeed / 2;
  int16_t leftFrontSpeed = leftSpeed - LR_dSpeed + FB_dSpeed;
  int16_t rightFrontSpeed = rightSpeed + LR_dSpeed + FB_dSpeed;
  int16_t leftBackSpeed = leftSpeed - LR_dSpeed - FB_dSpeed;
  int16_t rightBackSpeed = rightSpeed + LR_dSpeed - FB_dSpeed;

  speedAdjustmentForFour(leftFrontSpeed, leftBackSpeed, rightFrontSpeed, rightBackSpeed);
}

/// @brief 电机停止转动
/// @note 电机停止使用这个函数，为以后使用无刷电机做铺垫
void motorStop() {
  motorRunModeChange(MOTOR_MODE_Direct);
#ifdef FOUR_WHEEL
  speedAdjustmentForFour(0, 0, 0, 0);
#endif  // FOUR_WHEEL

#ifdef TWO_WHEEL
  speedAdjustment(0, 0);
#endif  // TWO_WHEEL
  gl_motor.leftSpeed = 0;
  gl_motor.rightSpeed = 0;
}

/// @brief 改变电机的驱动模式
/// @param mode 电机的驱动模式，目前有两种，一种是直接输出PWM，一种是固定转速模式
void motorRunModeChange(enum MotorMode mode) {
  switch (mode) {
    case MOTOR_MODE_Direct:
      MotorPIDAdjustObserver.update = NULL;
      for (int i = 0; i < 4; i++)
      {
        DEKE_ChangeMode(&Motor_handle[i],MOTOR_MODE_Direct);
      }
      break;
    case MOTOR_MODE_SPEED:
    if(MotorPIDAdjustObserver.update == NULL){
      MotorPIDAdjustObserver.update = SpeedAdjustment_PID;
      for (int i = 0; i < 4; i++)
      {
        DEKE_ChangeMode(&Motor_handle[i],MOTOR_MODE_SPEED);
      }


    // 限幅8000，比例2，积分8，微分0，主要靠积分,此参数适用于0.1秒调整一次
    // 限幅一定不能超过8k
//     PID_Init(pidLeftFront, gl_motor.leftSpeed, 6000, 0.08, 0.5, 0);   //左前轮
//     PID_Init(pidRightFront, gl_motor.rightSpeed, 6000, 0.08, 0.5, 0); //右前轮
// #ifdef FOUR_ENCODER
//     PID_Init(pidLeftBack, gl_motor.leftSpeed, 6000, 0.08, 0.5, 0);//左后轮
//     PID_Init(pidRightBack, gl_motor.rightSpeed, 6000, 0.08, 0.5, 0);//右后轮
// #endif  // FOUR_WHEEL
//     // 通过速度获取PWM值，用于初始化积分项,使得电机速度更快的到达目标速度
// #ifndef FOUR_ENCODER
//     pidLeftFront->sumError = getLeftMotorPWMBySpeed(gl_motor.leftSpeed) / pidLeftFront->ki;     // 左轮
//     pidRightFront->sumError = getRightMotorPWMBySpeed(gl_motor.rightSpeed) / pidRightFront->ki; // 右轮
// #endif  // !FOUR_WHEEL
// #ifdef FOUR_ENCODER
//     pidLeftFront->sumError = getLeftFrontMotorPWMBySpeed(gl_motor.leftSpeed) / pidLeftFront->ki;      //左前轮
//     pidLeftBack->sumError = getLeftBackMotorPWMBySpeed(gl_motor.leftSpeed) / pidLeftBack->ki;         //左后轮
//     pidRightFront->sumError = getRightFrontMotorPWMBySpeed(gl_motor.rightSpeed) / pidRightFront->ki;  //右前轮
//     pidRightBack->sumError = getRightBackMotorPWMBySpeed(gl_motor.rightSpeed) / pidRightBack->ki;     //右后轮
// #endif  // FOUR_WHEEL
    }
      break;
    default:
      break;
  }
}
enum MotorMode getMotorRunMode() {
  if (MotorPIDAdjustObserver.update == NULL) {
    return MOTOR_MODE_Direct;
  } else {
    return MOTOR_MODE_SPEED;
  }
}

// 编码器部分
/// @brief 获取编码器的值，用于计算速度和累计距离
/// @note
/// @note
/// 算出来的速度是每0.1秒的速度，累计距离是每0.1秒的累计距离，即使调用的频率不是0.1秒一次，也要这样算，才能用于PID调节
void GET_NUM() {
  if (DEKE_Encoder_Task()) {
#ifndef FOUR_ENCODER
    gl_motor.encoderLeftFront = DEKE_Get_encoder(&Motor_handle[LEFT_FRONT]);
    gl_motor.encoderRightFront = DEKE_Get_encoder(&Motor_handle[RIGHT_FRONT]);
    gl_motor.encoderDistance += (gl_motor.encoderLeftFront + gl_motor.encoderRightFront) / 2;
    gl_motor.encoderDistanceLeft += gl_motor.encoderLeftFront;
    gl_motor.encoderDistanceRight += gl_motor.encoderRightFront;
#else
    gl_motor.encoderLeftFront = DEKE_Get_encoder(&Motor_handle[LEFT_FRONT]);
    gl_motor.encoderLeftBack = DEKE_Get_encoder(&Motor_handle[LEFT_BACK]);
    gl_motor.encoderRightFront = DEKE_Get_encoder(&Motor_handle[RIGHT_FRONT]);
    gl_motor.encoderRightBack = DEKE_Get_encoder(&Motor_handle[LEFT_BACK]);
    gl_motor.encoderDistance += ((int_least16_t)gl_motor.encoderLeftFront +
                                 (int_least16_t)gl_motor.encoderLeftBack +
                                 (int_least16_t)gl_motor.encoderRightFront +
                                 (int_least16_t)gl_motor.encoderRightBack) /
                                4;
#endif    
  }
}
Observer encoderNumObserver = {.id = 1, .update = GET_NUM};  // 编码器中断观察者
void EncoderInit() {
  HAL_TIM_Encoder_Start(&LEFT_FRONT_ENCODER_TIM_HANDLE, TIM_CHANNEL_ALL);
  HAL_TIM_Encoder_Start(&RIGHT_FRONT_ENCODER_TIM_HANDLE, TIM_CHANNEL_ALL);
#ifdef FOUR_ENCODER
  HAL_TIM_Encoder_Start(&LEFT_BACK_ENCODER_TIM_HANDLE, TIM_CHANNEL_ALL);
  HAL_TIM_Encoder_Start(&RIGHT_BACK_ENCODER_TIM_HANDLE, TIM_CHANNEL_ALL);
#endif  // !TWO_WHEEL
  gl_TIM6_IT.add(&gl_TIM6_IT, &encoderNumObserver);
}




/*-------------Test--------------*/
#include "display.h"
#include "ABS_key.h"

/// @brief 直接输出PWM
/// @param key_Num
/// @return
#ifdef FOUR_WHEEL
My_StatusTypeDef PWMTest(int key_Num) {
  static uint8_t flag = 0;
  if (flag == 0) {
    flag = 1;
    motorRunModeChange(MOTOR_MODE_Direct);
    displayStr("LF_speed:", Display_firstCol+Display_OneChar, Display_firstRow, RED);
    displayStr("RF_speed:", Display_firstCol+Display_OneChar, Display_secondRow, RED);
    displayStr("LB_speed:", Display_firstCol+Display_OneChar, Display_thirdRow, RED);
    displayStr("RB_speed:", Display_firstCol+Display_OneChar, Display_fourthRow, RED);
    displayStr(">", Display_firstCol, Display_firstRow, RED);
  }
  displayNum(gl_motor.encoderLeftFront, Display_firstCol + 8 * 10,
             Display_firstRow);
  displayNum(gl_motor.encoderRightFront, Display_firstCol + 8 * 10,
             Display_secondRow);
#ifdef FOUR_ENCODER
  displayNum(gl_motor.encoderLeftBack, Display_firstCol + 8 * 10,
              Display_thirdRow);
  displayNum(gl_motor.encoderRightBack, Display_firstCol + 8 * 10,
              Display_fourthRow);
#else
  displayStr("noData", Display_firstCol + Display_OneChar*10, Display_thirdRow, RED);
  displayStr("noData", Display_firstCol + Display_OneChar*10, Display_fourthRow, RED);
#endif
  static int16_t wheel_speed_PWM[4] = {0};//左前，右前，左后，右后
  static uint8_t LR_flag = 0;
displayNum(wheel_speed_PWM[0],Display_firstCol, Display_fifthRow);
displayNum(wheel_speed_PWM[1],Display_firstCol+8*10,Display_fifthRow);
displayNum(wheel_speed_PWM[2],Display_firstCol, Display_fifthRow+Display_spaced);
displayNum(wheel_speed_PWM[3],Display_firstCol+8*10,Display_fifthRow+Display_spaced);
  if (key_Num == Key_ESC) {
    for (int i = 0; i < 4; i++) {
      wheel_speed_PWM[i] = 0;
    }
    LR_flag = 0;
    flag = 0;
    motorStop();
    return MY_READY;
  } else if (key_Num == Key_OK) {
    displayStr(" ", Display_firstCol, Display_firstRow+LR_flag*Display_spaced, RED);
    LR_flag++;
    LR_flag %= 4;
    displayStr(">", Display_firstCol, Display_firstRow+LR_flag*Display_spaced, RED);
    return MY_STANDBY;
  } else if (key_Num == Key_Down) {
    wheel_speed_PWM[LR_flag] -= 50;
    speedAdjustmentForFour(wheel_speed_PWM[0], wheel_speed_PWM[2], wheel_speed_PWM[1], wheel_speed_PWM[3]);
  } else if (key_Num == Key_UP) {
    wheel_speed_PWM[LR_flag] += 50;
    speedAdjustmentForFour(wheel_speed_PWM[0], wheel_speed_PWM[2], wheel_speed_PWM[1], wheel_speed_PWM[3]);
  }

  return MY_BUSY;
}
#endif // FOUR_WHEEL
#ifdef TWO_WHEEL
My_StatusTypeDef PWMTest(int key_Num) {
  static uint8_t flag = 0;
  if (flag == 0) {
    flag = 1;
    motorRunModeChange(MOTOR_MODE_Direct);
    displayStr("LSpeed:", Display_firstCol + 8, Display_firstRow, RED);
    displayStr("RSpeed:", Display_firstCol + 8, Display_secondRow, RED);
    displayStr(">", Display_firstCol, Display_firstRow, RED);
  }
  displayNum(gl_motor.encoderLeftFront, Display_firstCol + 8 * 10,
             Display_firstRow);
  displayNum(gl_motor.encoderRightFront, Display_firstCol + 8 * 10,
             Display_secondRow);
  displayNum(__HAL_TIM_GET_COMPARE(&LEFT_TIM_HANDLE, TIM_CHANNEL_3),
             Display_firstCol, Display_thirdRow);
  displayNum(__HAL_TIM_GET_COMPARE(&LEFT_TIM_HANDLE, TIM_CHANNEL_4),
             Display_firstCol, Display_fourthRow);
  displayNum(__HAL_TIM_GET_COMPARE(&RIGHT_TIM_HANDLE, TIM_CHANNEL_3),
             Display_firstCol + 8 * 10, Display_thirdRow);
  displayNum(__HAL_TIM_GET_COMPARE(&RIGHT_TIM_HANDLE, TIM_CHANNEL_4),
             Display_firstCol + 8 * 10, Display_fourthRow);
  static int16_t L_speed_PWM = 0;
  static int16_t R_speed_PWM = 0;
  static uint8_t LR_flag = 0;
  if (key_Num == Key_ESC) {
    L_speed_PWM = 0;
    R_speed_PWM = 0;
    LR_flag = 0;
    flag = 0;
    speedAdjustment(L_speed_PWM, R_speed_PWM);
    return MY_READY;
  } else if (key_Num == Key_OK) {
    if (LR_flag == 0) {
      LR_flag = 1;
      displayStr(" ", Display_firstCol, Display_firstRow, RED);
      displayStr(">", Display_firstCol, Display_secondRow, RED);
    } else {
      LR_flag = 0;
      displayStr(" ", Display_firstCol, Display_secondRow, RED);
      displayStr(">", Display_firstCol, Display_firstRow, RED);
    }
    return MY_STANDBY;
  } else if (key_Num == Key_Down) {
    if (LR_flag == 0)
      L_speed_PWM -= 50;
    else
      R_speed_PWM -= 50;
    speedAdjustment(L_speed_PWM, R_speed_PWM);
  } else if (key_Num == Key_UP) {
    if (LR_flag == 0)
      L_speed_PWM += 50;
    else
      R_speed_PWM += 50;
    speedAdjustment(L_speed_PWM, R_speed_PWM);
  }

  return MY_BUSY;
}
#endif // TWO_WHEEL

/// @brief 显示速度和距离
/// @param key_Num
/// @return
#ifdef FOUR_ENCODER
My_StatusTypeDef displaySpeed(int key_Num) {
  int32_t distance = gl_motor.encoderDistance;
  displayStr("LFSpeed:", Display_firstCol, Display_firstRow, RED);
  displayNum(gl_motor.encoderLeftFront, Display_firstCol + 10 * 8,
             Display_firstRow);
  displayStr("LBSpeed:", Display_firstCol, Display_secondRow, RED);
  displayNum(gl_motor.encoderLeftBack, Display_firstCol + 10 * 8,
             Display_secondRow);
  displayStr("RFSpeed:", Display_firstCol, Display_thirdRow, RED);
  displayNum(gl_motor.encoderRightFront, Display_firstCol + 10 * 8,
             Display_thirdRow);
  displayStr("RBSpeed:", Display_firstCol, Display_fourthRow, RED);
  displayNum(gl_motor.encoderRightBack, Display_firstCol + 10 * 8,
             Display_fourthRow);

  // displayStr("dis:", Display_firstCol, Display_fifthRow, RED);
  // displayNum(distance, Display_firstCol + 10 * 8, Display_fifthRow);
  if (key_Num == Key_ESC) {
    return MY_OK;
  }
  return MY_BUSY;
}
#else
My_StatusTypeDef displaySpeed(int key_Num) {
  int32_t distance = gl_motor.encoderDistance;
  displayStr("LSpeed:", Display_firstCol, Display_firstRow, RED);
  displayNum(gl_motor.encoderLeftFront, Display_firstCol + Display_OneChar * 8,
             Display_firstRow);
  displayStr("RSpeed:", Display_firstCol, Display_secondRow, RED);
  displayNum(gl_motor.encoderRightFront, Display_firstCol + Display_OneChar * 8,
             Display_secondRow);
  displayStr("dis:", Display_firstCol, Display_thirdRow, RED);
  displayNum(distance, Display_firstCol + Display_OneChar * 8, Display_thirdRow);


  if (key_Num == Key_ESC) {
    return MY_OK;
  }
  return MY_BUSY;
}
#endif  // FOUR_ENCODER


/// @brief 测试编码器和PWM,固定转速模式下
/// @param key_Num
/// @return
#ifdef FOUR_ENCODER

My_StatusTypeDef testEncodingAndPWM(int key_Num) {
  static int flag = 0;
  if (flag == 0) {
    flag = 1;
    motorRunModeChange(MOTOR_MODE_SPEED);
    displayStr("LFencoder:", Display_firstCol, Display_firstRow, RED);
    displayStr("RFSpeed:", Display_firstCol, Display_secondRow, RED);
  }
  // displayStr("LFencoder:", Display_firstCol, Display_firstRow, RED);
  // displayNum(gl_motor.encoderLeftFront, Display_firstCol + 8 * 10,
  //            Display_firstRow);
  // displayStr("PWM_ch3:", Display_firstCol, Display_secondRow, RED);
  // displayNum(__HAL_TIM_GET_COMPARE(&LEFT_TIM_HANDLE, TIM_CHANNEL_3),
  //            Display_firstCol + 8 * 10, Display_secondRow);
  // displayStr("PWM_ch4:", Display_firstCol, Display_thirdRow, RED);
  // displayNum(__HAL_TIM_GET_COMPARE(&LEFT_TIM_HANDLE, TIM_CHANNEL_4),
  //            Display_firstCol + 8 * 10, Display_thirdRow);

  displayNum(gl_motor.encoderLeftFront, Display_firstCol + 8 * 10,
             Display_firstRow);

  displayNum(gl_motor.encoderRightFront, Display_firstCol + 8 * 10,
             Display_secondRow);
  displayNum(__HAL_TIM_GET_COMPARE(&LEFT_TIM_HANDLE, TIM_CHANNEL_3),
             Display_firstCol, Display_thirdRow);
  displayNum(__HAL_TIM_GET_COMPARE(&LEFT_TIM_HANDLE, TIM_CHANNEL_4),
             Display_firstCol + 8 * 10, Display_thirdRow);
  displayNum(__HAL_TIM_GET_COMPARE(&RIGHT_TIM_HANDLE, TIM_CHANNEL_3),
             Display_firstCol, Display_fourthRow);
  displayNum(__HAL_TIM_GET_COMPARE(&RIGHT_TIM_HANDLE, TIM_CHANNEL_4),
             Display_firstCol + 8 * 10, Display_fourthRow);
  static uint8_t lr_flag = 0;
  if (key_Num == Key_ESC) {
    flag = 0;  // 退出时，把标志位改成0
    lr_flag = 0;
    // MotorPIDAdjustObserver.update = NULL;  //
    // 退出时，把观察者的update函数改成空
    motorRunModeChange(MOTOR_MODE_Direct);  // 退出时，把电机模式改成直接模式
    return MY_READY;
  } else if (key_Num == Key_OK) {
    // resetMotorSpeedValue_runTime(0, 0);
    if (lr_flag == 0)
      lr_flag = 1;
    else
      lr_flag = 0;
    return MY_STANDBY;
  } else if (key_Num == Key_Down) {
    if (lr_flag == 0)
      resetMotorSpeedValue_runTime(gl_motor.leftSpeed - 50,
                                   gl_motor.rightSpeed);
    else
      resetMotorSpeedValue_runTime(gl_motor.leftSpeed,
                                   gl_motor.rightSpeed - 50);
    // resetMotorSpeedValue_runTime(gl_motor.leftSpeed - 50,
    // gl_motor.rightSpeed);
  } else if (key_Num == Key_UP) {
    if (lr_flag == 0)
      resetMotorSpeedValue_runTime(gl_motor.leftSpeed + 50,
                                   gl_motor.rightSpeed);
    else
      resetMotorSpeedValue_runTime(gl_motor.leftSpeed,
                                   gl_motor.rightSpeed + 50);
    // resetMotorSpeedValue_runTime(gl_motor.leftSpeed + 50,
    // gl_motor.rightSpeed);
  }

  return MY_BUSY;
}
#else
My_StatusTypeDef testEncodingAndPWM(int key_Num) {
  static int flag = 0;
  if (flag == 0) {
    flag = 1;
    motorRunModeChange(MOTOR_MODE_SPEED);
    displayStr("LSpeed:", Display_firstCol + 8, Display_firstRow, RED);
    displayStr("RSpeed:", Display_firstCol + 8, Display_secondRow, RED);
    displayStr(">", Display_firstCol, Display_firstRow, RED);
  }
  displayNum(gl_motor.encoderLeftFront, Display_firstCol + 8 * Display_OneChar,
             Display_firstRow);

  displayNum(gl_motor.encoderRightFront, Display_firstCol + 8 * Display_OneChar,
             Display_secondRow);
  displayNum(__HAL_TIM_GET_COMPARE(&LEFT_TIM_HANDLE, TIM_CHANNEL_3),
             Display_firstCol, Display_thirdRow);
  displayNum(__HAL_TIM_GET_COMPARE(&LEFT_TIM_HANDLE, TIM_CHANNEL_4),
             Display_firstCol + 8 * Display_OneChar, Display_thirdRow);
  displayNum(__HAL_TIM_GET_COMPARE(&RIGHT_TIM_HANDLE, TIM_CHANNEL_3),
             Display_firstCol, Display_fourthRow);
  displayNum(__HAL_TIM_GET_COMPARE(&RIGHT_TIM_HANDLE, TIM_CHANNEL_4),
             Display_firstCol + 8 * Display_OneChar, Display_fourthRow);
  static uint8_t lr_flag = 0;
  if (key_Num == Key_ESC) {
    flag = 0;  // 退出时，把标志位改成0
    lr_flag = 0;
    // MotorPIDAdjustObserver.update = NULL;  //
    // 退出时，把观察者的update函数改成空
    motorRunModeChange(MOTOR_MODE_Direct);  // 退出时，把电机模式改成直接模式
    return MY_READY;
  } else if (key_Num == Key_OK) {
    if (lr_flag == 0) {
      lr_flag = 1;
      displayStr(" ", Display_firstCol, Display_firstRow, RED);
      displayStr(">", Display_firstCol, Display_secondRow, RED);
    } else {
      lr_flag = 0;
      displayStr(" ", Display_firstCol, Display_secondRow, RED);
      displayStr(">", Display_firstCol, Display_firstRow, RED);
    }
    return MY_STANDBY;
  } else if (key_Num == Key_Down) {
    if (lr_flag == 0)
      resetMotorSpeedValue_runTime(gl_motor.leftSpeed - 50,
                                   gl_motor.rightSpeed);
    else
      resetMotorSpeedValue_runTime(gl_motor.leftSpeed,
                                   gl_motor.rightSpeed - 50);
    // resetMotorSpeedValue_runTime(gl_motor.leftSpeed - 50,
    // gl_motor.rightSpeed);
  } else if (key_Num == Key_UP) {
    if (lr_flag == 0)
      resetMotorSpeedValue_runTime(gl_motor.leftSpeed + 50,
                                   gl_motor.rightSpeed);
    else
      resetMotorSpeedValue_runTime(gl_motor.leftSpeed,
                                   gl_motor.rightSpeed + 50);
    // resetMotorSpeedValue_runTime(gl_motor.leftSpeed + 50,
    // gl_motor.rightSpeed);
  }

  return MY_BUSY;
}
#endif // FOUR_ENCODER

#include "Mydelay.h"
/**
 * @brief 让小车跑1m，用于校正编码器的值
 * 
 * @param key_Num 
 * @return My_StatusTypeDef 
 * @note 用尺子测出实际走的距离:Rd（单位mm）,编码器的差，即显示的值为dis，则C=dis/Rd，然后把编码器的值（LEFT（Right）_FRONT_ENCODER_ONE_ROUND）乘以C，就是实际的值
 * 比如：车实际走了1.2米，编码器显示的值是1010，那么C=1010/1200=0.842，那么把LEFT_FRONT_ENCODER_ONE_ROUND和RIGHT_FRONT_ENCODER_ONE_ROUND都乘以0.842就是实际的值
 * @note 新增了左右轮的编码器值，用于分别校正左右轮的编码器值，算法同上
 * @note 如果车走不直，可以调整直行时左右轮子的PWM
 */
My_StatusTypeDef testOneMetre(int key_Num) {
  static int32_t distance = 0;
#ifndef FOUR_ENCODER
  static int32_t distanceLeft = 0;
  static int32_t distanceRight = 0;
#endif
#ifdef FOUR_ENCODER
  static int32_t distanceLeftFront = 0;
  static int32_t distanceRightFront = 0;
  static int32_t distanceLeftBack = 0;
  static int32_t distanceRightBack = 0;
#endif
  static uint8_t flag = 0;
  if (flag == 1 && key_Num == Key_ESC) {
    flag = 0;
    motorStop();
    return MY_READY;
  } else if (flag == 1 && key_Num == Key_OK) {
    flag = 2;
  }
  if (flag == 0) {
    displayStr("press OK to run 1m ", Display_firstCol, Display_firstRow, RED);
    flag = 1;
  } else if (flag == 2) {
    flag = 3;
    motorRunModeChange(MOTOR_MODE_Direct);
    //记录当前的编码器值
    distance = gl_motor.encoderDistance;
#ifndef FOUR_ENCODER
    distanceLeft = gl_motor.encoderDistanceLeft;
    distanceRight = gl_motor.encoderDistanceRight;
#endif
#ifdef FOUR_ENCODER
    distanceLeftFront = gl_motor.encoderLeftFront;
    distanceRightFront = gl_motor.encoderRightFront;
    distanceLeftBack = gl_motor.encoderLeftBack;
    distanceRightBack = gl_motor.encoderRightBack;
#endif
    speedAdjustment(1000, 1000);
  } else if (flag == 3 &&
             gl_motor.encoderDistance - distance >= 1000 * ENCODE_OF_ONE_mm) {
    motorStop();
    //等待电机真正停下来
    if (myDelay((uint32_t)testOneMetre, 500)) {
      displayStr("distance:", Display_firstCol, Display_secondRow, RED);
      char str[20];
      sprintf(str, "dis:%.2fm  ",
              (gl_motor.encoderDistance - distance) / ENCODE_OF_ONE_mm);
      displayStr(str, Display_firstCol, Display_thirdRow, RED);
#ifndef FOUR_ENCODER
      sprintf(str, "L_dis:%.2fm  ",
              (gl_motor.encoderDistanceLeft - distanceLeft) / ENCODE_OF_ONE_mm);
      displayStr(str, Display_firstCol, Display_fourthRow, RED);
      sprintf(str, "R_dis:%.2fm  ",
              (gl_motor.encoderDistanceRight - distanceRight) / ENCODE_OF_ONE_mm);
      displayStr(str, Display_firstCol, Display_fifthRow, RED);
      displayStr("press ESC to exit", Display_firstCol, Display_fifthRow+Display_spaced, RED);
#endif
#ifdef FOUR_ENCODER
      //没用到，先不写
#endif
      flag = 1;
    }
  }
  return MY_BUSY;
}